#include "remote_vehicle.h"
#include "muduo/base/Timestamp.h"
#include "utils.h"

#include <iomanip>

using namespace decision::adas;

RemoteVehicle::RemoteVehicle(int64_t id)
    : Vehicle(id)
{
}

void RemoteVehicle::calc_relation(const Vehicle& host)
{
    std::lock_guard<std::mutex> guard(lock_);

    muduo::Timestamp now = muduo::Timestamp::now();

    azimuth_ = host.azimuth(*this) - host.heading();

    distance_ = distances_[now.microSecondsSinceEpoch()] = this->distance(host);

    distances_xy_[now.microSecondsSinceEpoch()] = decision::math::Vec2d(x(), y());

    { // 计算 移动方向
        auto iter = distances_xy_.upper_bound(muduo::addTime(now, -5).microSecondsSinceEpoch());
        if (iter != distances_xy_.end()) {
            decision::math::Segment2d vec(iter->second, distances_xy_.rbegin()->second);
            auto hcenter = decision::math::Vec2d(host.x(), host.y());
            decision::math::Vec2d foot;
            predicted_min_distance_ = vec.get_perpendicular_foot(hcenter, &foot);
        } else {
            predicted_min_distance_ = NAN;
        }
    }

    { // 计算TTC
        auto iter = distances_.upper_bound(muduo::addTime(now, -5).microSecondsSinceEpoch());
        if (iter != distances_.end()) {
            auto time_diff = distances_.rbegin()->first - iter->first;
            auto dist_diff = iter->second - distances_.rbegin()->second;
            rel_speed_ = dist_diff / time_diff * 1000 * 1000;
            ttc_ = distance_ / rel_speed_;
        } else {
            rel_speed_ = NAN;
            ttc_ = NAN;
        }
    }

    { // delete data before 10s
        auto iter = distances_.upper_bound(muduo::addTime(now, -10).microSecondsSinceEpoch());
        if (iter != distances_.end()) {
            distances_.erase(distances_.begin(), iter);
        }
    }
}

std::string RemoteVehicle::relation_debug_string() const
{
    std::stringstream ss;
    ss << std::fixed << std::setprecision(2)
       << "tid:" << trackid()
       << " azimuth:" << azimuth_ / M_PI * 180 << "/" << legalize_angle(-azimuth_ / M_PI * 180)
       << " distance:" << distance_
       << " rel_speed:" << rel_speed_
       << " ttc:" << ttc_
       << " predicted_min_distance_:" << predicted_min_distance_;
    return ss.str();
}

double RemoteVehicle::get_relative_distance()
{
    return distance_;
}

double RemoteVehicle::get_relative_azimuth()
{
    return azimuth_;
}

double RemoteVehicle::get_ttc()
{
    return ttc_;
}

double RemoteVehicle::get_predicted_min_distance()
{
    return predicted_min_distance_;
}